#include "S_PID.h"
#include "math.h"

static void limit(float *ori, float max)
{
	if (*ori > max)
		*ori = max;
	if (*ori < -max)
		*ori = -max;
}

/**
 * @brief 初始化pid结构体
 * 
 * @param pid pid结构体地址
 * @param dt 调用pid计算的时间间隔，单位为秒
 * @param I_limit 积分限幅
 * @param I_band 积分分离
 * @param out_limit 输出限幅
 */
void PIDInit(PID_t *pid, float dt, float I_limit, float I_band, float out_limit)
{
	pid->dt = dt;
	pid->i_limit = I_limit;
	pid->i_band = I_band;
	pid->OUT_limit = out_limit;
	pid->e = 0;
	pid->last_e = 0;
	pid->i_out = 0;
	pid->d_out = 0;
}
/**
 * @brief 设置pid三个参数
 * 
 * @param pid pid结构体地址
 * @param p 比例
 * @param i 积分
 * @param d 微分
 */
void PIDConfig(PID_t *pid, float p, float i, float d)
{
	pid->kp = p;
	pid->ki = i;
	pid->kd = d;
}
/**
 * @brief 设置目标值
 * 
 * @param pid pid结构体地址
 * @param target 目标值
 */
void PIDSetTarget(PID_t *pid, float target)
{
	pid->target = target;
}

/**
 * @brief 计算pid
 * 
 * @param pid pid结构体地址
 */
void PID_Calc(PID_t *pid)
{
	pid->e = pid->target - pid->current; //error计算

	//P计算
	pid->p_out = pid->kp * pid->e;

	//I计算
	if (fabsf(pid->e) < pid->i_band) //积分分离
	{
		pid->i_out += (pid->ki * pid->e) * pid->dt;
		limit(&(pid->i_out), pid->i_limit); //积分限幅
	}
	else
		pid->i_out = 0;

	//D计算
	pid->d_out = pid->kd * (pid->e - pid->last_e) / pid->dt;

	//PID总输出
	pid->OUT = pid->p_out + pid->i_out + pid->d_out;
	//PID输出限幅
	limit(&(pid->OUT), pid->OUT_limit);

	pid->last_e = pid->e;
}

float yaw_calc_plus(float num1, float num2)
{
	float res = num1 + num2;
	if (res > 180)
		res = res - 360;
	else if (res < -180)
		res = 360 + res;

	return res;
}

void PID_Calc_yaw(PID_t *pid)
{
	pid->e = yaw_calc_plus(pid->target,-pid->current);
	// pid->e = pid->target - pid->current; //error计算

	//P计算
	pid->p_out = pid->kp * pid->e;

	//I计算
	if (fabsf(pid->e) < pid->i_band) //积分分离
	{
		pid->i_out += (pid->ki * pid->e) * pid->dt;
		limit(&(pid->i_out), pid->i_limit); //积分限幅
	}
	else
		pid->i_out = 0;

	//D计算
	pid->d_out = pid->kd * (pid->e - pid->last_e) / pid->dt;

	//PID总输出
	pid->OUT = pid->p_out + pid->i_out + pid->d_out;
	//PID输出限幅
	limit(&(pid->OUT), pid->OUT_limit);

	pid->last_e = pid->e;
}